/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-11 13:52:35
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-01 03:05:53
 * @Description: ...
 * @Reference: ...
 */

#include "gps_converter.h"

namespace kaist_publisher {

GpsConverter::GpsConverter(ros::NodeHandle &nh, const std::string &dataset_dir,
                           const std::string &save_dir,
                           const std::string &topic)
    : Converter(dataset_dir, save_dir), topic_(topic) {
    bag_name_ = FilterSlash(topic_) + ".bag";
    pub_gps_ = nh.advertise<sensor_msgs::NavSatFix>(topic_.c_str(), 2);
}

int64_t GpsConverter::Init_Publish() {
    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    fp_ = fopen(data_file.c_str(), "r");

    if (fscanf(fp_, "%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",
               &stamp_, &latitude_, &longitude_, &altitude_, &cov_[0], &cov_[1],
               &cov_[2], &cov_[3], &cov_[4], &cov_[5], &cov_[6], &cov_[7],
               &cov_[8]) == 13) {
        return stamp_;
    } else {
        fclose(fp_);
        return LONG_MAX;
    }
}

int64_t GpsConverter::Publish() {
    sensor_msgs::NavSatFix gps_data;
    gps_data.header.stamp.fromNSec(stamp_);
    gps_data.header.frame_id = "gps";
    gps_data.latitude = latitude_;
    gps_data.longitude = longitude_;
    gps_data.altitude = altitude_;
    for (int i = 0; i < 9; ++i) {
        gps_data.position_covariance[i] = cov_[i];
    }
    // pub_gps_.publish(gps_data);

    if (fscanf(fp_, "%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",
               &stamp_, &latitude_, &longitude_, &altitude_, &cov_[0], &cov_[1],
               &cov_[2], &cov_[3], &cov_[4], &cov_[5], &cov_[6], &cov_[7],
               &cov_[8]) == 13) {
        return stamp_;
    }

    fclose(fp_);
    return LONG_MAX;
}

void GpsConverter::Publish(int64_t &timestamp,
                           sensor_msgs::NavSatFix::Ptr &gps_data) {
    gps_data.reset(new sensor_msgs::NavSatFix());
    gps_data->header.stamp.fromNSec(stamp_);
    gps_data->header.frame_id = "gps";
    gps_data->latitude = latitude_;
    gps_data->longitude = longitude_;
    gps_data->altitude = altitude_;
    for (int i = 0; i < 9; ++i) {
        gps_data->position_covariance[i] = cov_[i];
    }
    // pub_gps_.publish(gps_data);

    if (fscanf(fp_, "%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",
               &stamp_, &latitude_, &longitude_, &altitude_, &cov_[0], &cov_[1],
               &cov_[2], &cov_[3], &cov_[4], &cov_[5], &cov_[6], &cov_[7],
               &cov_[8]) == 13) {
        timestamp = stamp_;
        return;
    }

    fclose(fp_);
    timestamp = LONG_MAX;
    return;
}

int GpsConverter::Convert() {
    CheckAndCreateSaveDir();

    boost::filesystem::path bag_file =
        boost::filesystem::path(save_dir_) / bag_name_;
    // rosbag::Bag bag(bag_file.string(), rosbag::bagmode::Write);
    // ROS_INFO("saving %s", bag_file.c_str());

    const std::string data_file = dataset_dir_ + "/" + default_data_file;
    FILE *fp = fopen(data_file.c_str(), "r");
    sensor_msgs::NavSatFix gps_data;
    int64_t stamp;
    double latitude, longitude, altitude;
    double cov[9];
    while (fscanf(fp, "%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",
                  &stamp, &latitude, &longitude, &altitude, &cov[0], &cov[1],
                  &cov[2], &cov[3], &cov[4], &cov[5], &cov[6], &cov[7],
                  &cov[8]) == 13) {
        gps_data.header.stamp.fromNSec(stamp);
        gps_data.header.frame_id = "gps";
        gps_data.latitude = latitude;
        gps_data.longitude = longitude;
        gps_data.altitude = altitude;
        for (int i = 0; i < 9; ++i) {
            gps_data.position_covariance[i] = cov[i];
        }

        // bag.write(topic_, gps_data.header.stamp, gps_data);
        pub_gps_.publish(gps_data);
    }
    // bag.close();
    fclose(fp);
    // ROS_INFO("done saving %s", bag_file.c_str());

    return 0;
}

}  // namespace kaist_publisher